/*    rpg_quadrotor_mpc
 *    A model predictive control implementation for quadrotors.
 *    Copyright (C) 2017-2018 Philipp Foehn, 
 *    Robotics and Perception Group, University of Zurich
 * 
 *    Intended to be used with rpg_quadrotor_control and rpg_quadrotor_common.
 *    https://github.com/uzh-rpg/rpg_quadrotor_control
 *
 *    This program is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    This program is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */


#include <memory>
#include <acado_optimal_control.hpp>
#include <acado_code_generation.hpp>
#include <acado_gnuplot.hpp>

// Standalone code generation for a parameter-free quadrotor model
// with thrust and rates input. 

const int pc_size=30;


int main( ){
  // Use Acado
  USING_NAMESPACE_ACADO

  /*
  Switch between code generation and analysis.

  If CODE_GEN is true the system is compiled into an optimizaiton problem
  for real-time iteration and all code to run it online is generated.
  Constraints and reference structure is used but the values will be set on
  runtinme.

  If CODE_GEN is false, the system is compiled into a standalone optimization
  and solved on execution. The reference and constraints must be set in here.
  */
  const bool CODE_GEN = !false;
  const bool USE_OD=!false;

  const int pc_size=30;


  // System variables
  DifferentialState     p_yaw,p_pitch;
  DifferentialState     v_yaw,v_pitch;


  Control               a_yaw,a_pitch;
  DifferentialEquation  f;
  Function              h, hN;
  //OnlineData            p_x,p_y,p_z;
  OnlineData            fx[pc_size],fy[pc_size],fz[pc_size];//position of features
  




  // Parameters with exemplary values. These are set/overwritten at runtime.
  const double t_start = 0.0;     // Initial time [s]
  const double t_end = 2.0;       // Time horizon [s]
  const double dt = 0.1;          // Discretization time [s]
  const int N = round(t_end/dt);  // Number of nodes
  const double g_z = 9.8066;      // Gravity is everywhere [m/s^2]
  const double w_max_yaw = 1;     // Maximal yaw rate [rad/s]
  const double w_max_xy = 3;      // Maximal pitch and roll rate [rad/s]
  const double T_min = 2;         // Minimal thrust [N]
  const double T_max = 20;        // Maximal thrust [N]

  // Bias to prevent division by zero.
  const double epsilon = 0.1;     // Camera projection recover bias [m]


  // System Dynamics

  f << dot(p_yaw) ==  v_yaw;
  f << dot(p_pitch) ==  v_pitch;
  f << dot(v_yaw) ==  a_yaw;
  f << dot(v_pitch) ==  a_pitch;
  

 
  IntermediateState pc_W(4,pc_size);
  IntermediateState p_B_x,p_B_y,p_B_z;
  
  if(USE_OD){
    for(int i=0;i<pc_size;++i){
      pc_W(0,i)=fx[i];
      pc_W(1,i)=fy[i];
      pc_W(2,i)=fz[i];
      pc_W(3,i)=1.0;
    }
  }
  else{
    for(int i=0;i<pc_size;++i){
      pc_W(0,i)=2.0;
      pc_W(1,i)=2.01;
      pc_W(2,i)=-1.0;
      pc_W(3,i)=1.0;
    }
    
  }

  IntermediateState T_wb(4,4),T_bc(4,4),T_wc(4,4);

    //T_wb


    T_wb(0,3) = 0.0;
    T_wb(1,3) = 0.0;
    T_wb(2,3) = 0.0;
    T_wb(3,3) = 1.0;

    T_wb(0,0) = cos(p_yaw);  T_wb(0,1)=sin(p_yaw); T_wb(0,2)=0;
    T_wb(1,0) = -sin(p_yaw); T_wb(1,1)=cos(p_yaw); T_wb(1,2)=0;
    T_wb(2,0) = 0; T_wb(2,1)=0; T_wb(2,2)=1.0;

  
    //T_bc
    T_bc(3,3)=1.0;
    T_bc(0,0) = cos(p_pitch);  T_bc(0,1)=0; T_bc(0,2)=sin(p_pitch);
    T_bc(1,0) = 0;               T_bc(1,1)=1; T_bc(1,2)=0;
    T_bc(2,0) = -sin(p_pitch); T_bc(2,1)=0; T_bc(2,2)=cos(p_pitch);

    T_wc=T_wb*T_bc;

    IntermediateState T_cw=T_wc.transpose();
    T_cw(3,0)=0.0;
    T_cw(3,1)=0.0;
    T_cw(3,2)=0.0;
    T_cw(0,3)=-T_wc(0,3);
    T_cw(1,3)=-T_wc(1,3);
    T_cw(2,3)=-T_wc(2,3);
   
    IntermediateState A=T_wb;
   //IntermediateState P=T_wc;
  IntermediateState perception_cost1=visibility_pc;
  IntermediateState perception_cost2=visibility_pc;

  h <<v_yaw<<v_pitch
   //IntermediateState detP = P(0,0)*(P(1,1)*P(2,2) - P(1,2)*P(2,1)) - P(0,1)*(P(1,0)*P(2,2) - P(1,2)*P(2,0)) + P(0,2)*(P(1,0)*P(2,1) - P(1,1)*P(2,0));

   IntermediateState pc_C=T_cw*pc_W;


   IntermediateState pc_C_2=pow(pc_C,2);
   IntermediateState dist_pc=sqrt(pc_C_2.getRow(0)+pc_C_2.getRow(1)+pc_C_2.getRow(2));
   IntermediateState cosb_pc=pc_C.getRow(0)/dist_pc;
   IntermediateState bear_pc=acos(cosb_pc);
   //IntermediateState visibility_pc=exp(-0.5*cosb_pc);
   //calculate visibility cost of each pc
   IntermediateState visibility_pc;
   IntermediateState a=0.1;
   IntermediateState half_fov=3.1415926/2;
   for(int p=0;p<pc_size;++p){
    
    //visibility_pc+=1/(1+exp(-a*cos(bear_pc(0,p)/half_fov)));
    visibility_pc+=((bear_pc(0,p))*(bear_pc(0,p)))*dist_pc(0,p);
   }

   visibility_pc=visibility_pc/pc_size;
 


  // Intermediate states to calculate point of interest projection!

  // Cost: Sum(i=0, ..., N-1){h_i' * Q * h_i} + h_N' * Q_N * h_N
  // Running cost vector consists of all states and inputs.
    << perception_cost1 << perception_cost2 
    << a_yaw<<a_pitch;

  // End cost vector consists of all states (no inputs at last state).
  hN <<v_yaw<<v_pitch
    << perception_cost1 << perception_cost2;

  // Running cost weight matrix
  DMatrix Q(h.getDim(), h.getDim());
  Q.setIdentity();
  Q(0,0) = 100;   // v_yaw
  Q(1,1) = 100;   // v_pitch
  Q(2,2) = 1000;   // perception_cost1
  Q(3,3) = 1000;   // perception_cost2
  Q(4,4) = 1;   // a_yaw
  Q(5,5) = 1;   // a_pitch



  // End cost weight matrix
  DMatrix QN(hN.getDim(), hN.getDim());
  QN.setIdentity();
  QN(0,0) = Q(0,0);   // v_yaw
  QN(1,1) = Q(1,1);   // v_pitch
  QN(2,2) = Q(2,2);   // perception_cost1
  QN(3,3) = Q(3,3);   // perception_cost2



  // Set a reference for the analysis (if CODE_GEN is false).
  // Reference is at x = 2.0m in hover (qw = 1).
  DVector r(h.getDim());    // Running cost reference
  r.setZero();

  

  DVector rN(hN.getDim());   // End cost reference
  rN.setZero();



  // DEFINE AN OPTIMAL CONTROL PROBLEM:
  // ----------------------------------
  OCP ocp( t_start, t_end, N );
  if(!CODE_GEN)
  {
    // For analysis, set references.
    ocp.minimizeLSQ( Q, h, r );
    ocp.minimizeLSQEndTerm( QN, hN, rN );
  }else{
    // For code generation, references are set during run time.
    BMatrix Q_sparse(h.getDim(), h.getDim());
    Q_sparse.setIdentity();
    BMatrix QN_sparse(hN.getDim(), hN.getDim());
    QN_sparse.setIdentity();
    ocp.minimizeLSQ( Q_sparse, h);
    ocp.minimizeLSQEndTerm( QN_sparse, hN );
  }

  // Add system dynamics
  ocp.subjectTo( f );
  // Add constraints


  ocp.setNOD(pc_size*3+3);


  if(!CODE_GEN)
  {
    // Set initial state
    ocp.subjectTo( AT_START, p_yaw ==  0.0 );
    ocp.subjectTo( AT_START, p_pitch ==  0.0 );
    ocp.subjectTo( AT_START, v_yaw ==  0.0 );
    ocp.subjectTo( AT_START, v_pitch ==  0.0 );


        // Setup some visualization
    GnuplotWindow window1( PLOT_AT_EACH_ITERATION );
    window1.addSubplot( p_pitch/3.14*180,"ppitch" );
    window1.addSubplot( p_yaw/3.14*180,"pyaw" );
    window1.addSubplot( visibility_pc,"pyaw" );

    GnuplotWindow window3( PLOT_AT_EACH_ITERATION );
    window3.addSubplot( A(0,0),"ppitch" );
    window3.addSubplot( A(0,1),"ppitch" );
    window3.addSubplot( A(0,2),"ppitch" );
    window3.addSubplot( A(1,0),"ppitch" );
    window3.addSubplot( A(1,1),"ppitch" );
    window3.addSubplot( A(1,2),"ppitch" );
    window3.addSubplot( A(2,0),"ppitch" );
    window3.addSubplot( A(2,1),"ppitch" );
    window3.addSubplot( A(2,2),"ppitch" );


    // Define an algorithm to solve it.
    OptimizationAlgorithm algorithm(ocp);
    algorithm.set( INTEGRATOR_TOLERANCE, 1e-1 );
    algorithm.set( KKT_TOLERANCE, 1e-6 );
    algorithm << window1;
    //algorithm << window3;
    algorithm.solve();

  }else{
    // For code generation, we can set some properties.
    // The main reason for a setting is given as comment.
    OCPexport mpc(ocp);

    mpc.set(HESSIAN_APPROXIMATION,  GAUSS_NEWTON);        // is robust, stable
    mpc.set(DISCRETIZATION_TYPE,    MULTIPLE_SHOOTING);   // good convergence
    mpc.set(SPARSE_QP_SOLUTION,     FULL_CONDENSING_N2);  // due to qpOASES
    mpc.set(INTEGRATOR_TYPE,        INT_IRK_GL4);         // accurate
    mpc.set(NUM_INTEGRATOR_STEPS,   N);
    mpc.set(QP_SOLVER,              QP_QPOASES);          // free, source code
    mpc.set(HOTSTART_QP,            YES);
    mpc.set(CG_USE_OPENMP,                    YES);       // paralellization
    mpc.set(CG_HARDCODE_CONSTRAINT_VALUES,    NO);        // set on runtime
    mpc.set(CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);       // time-varying costs
    mpc.set( USE_SINGLE_PRECISION,        YES);           // Single precision

    // Do not generate tests, makes or matlab-related interfaces.
    mpc.set( GENERATE_TEST_FILE,          NO);
    mpc.set( GENERATE_MAKE_FILE,          NO);
    mpc.set( GENERATE_MATLAB_INTERFACE,   NO);
    mpc.set( GENERATE_SIMULINK_INTERFACE, NO);

    // Finally, export everything.
    if(mpc.exportCode("quadrotor_mpc_codegen") != SUCCESSFUL_RETURN)
      exit( EXIT_FAILURE );
    mpc.printDimensionsQP( );
  }

  return EXIT_SUCCESS;
}